#pragma once

#include "types.h"

#include <stdminmax>
#include <algorithm>
#include <limits>
#include <xtd/tree2d.h> // static_typecast

//!dependency from mtx library of this common code is very undesired and makes things slower
//#include <mtx/fixed.h>
//#include <mtx/algorithm.h>

namespace g2 {

enum layout_code {
	inside		= 0,
	left		= 1,
	right		= 2,	
	below		= 4,
	above		= 8
};

template <typename T>
inline layout_code layout(basic_vector<T> v, basic_vector<T> v1, basic_vector<T> v2, T * ph = 0, T * ps = 0) 
{
	unsigned int c = inside;
	v2 -= v1;
	v -= v1;
	T v2v2 = scalar_product(v2, v2);
	T h = (v2.y * v.x - v2.x * v.y) / sqrt(v2v2);
	T s = scalar_product(v2, v) / v2v2;
	if (h > 0)
		c |= right;
	if (h < 0)
		c |= left;
	if (s < 0)
		c |= below;
	if (s > 1)
		c |= above;
	if (ph != 0)
		* ph = h;
	if (ps != 0)
		* ps = s;
	return layout_code(c);
}

template <typename T>
inline layout_code layout(basic_vector<T> v, const basic_box<T> & b) {
	unsigned int c = inside;
	if (v.x < b.x1)
		c |= left;
	if (v.y < b.y1)
		c |= below;
	if (v.x > b.x2)
		c |= right;
	if (v.y > b.y2)
		c |= above;
	return layout_code(c);
}

template <typename T>
inline T distance(basic_vector<T> v, basic_vector<T> v1, basic_vector<T> v2, T * ph = 0, T * ps = 0)
{
	T h, s, d;
	layout_code c = layout(v, v1, v2, &h, &s);
	if (c & below) {
		d = T(abs(v - v1));
		s = 0;
	} else if (c & above) {
		d = T(abs(v - v2));
		s = 1;
	} else {
		d = fabs(h);
	}
	if (ph != 0)
		* ph = h;
	if (ps != 0)
		* ps = s;
	return d;
}

template <typename T>
bool intersect(const basic_box<T> & b1, const basic_box<T> & b2, basic_box<T> * r = 0)
{
	bool result = 
		(b1.x1 < b2.x2) && (b2.x1 < b1.x2) &&
		(b1.y1 < b2.y2) && (b2.y1 < b1.y2);
	if (result && r) {
		r->x1 = std::max(b1.x1, b2.x1);
		r->x2 = std::min(b1.x2, b2.x2);
		r->y1 = std::max(b1.y1, b2.y1);
		r->y2 = std::min(b1.y2, b2.y2);
	}
	return result;
}

/// returns true if segments (v1-v2) & (u1-u2) intersect,
/// optionally returns point of intersection in the last argument
template <typename T>
bool intersect(const basic_vector<T> & v1, const basic_vector<T> & v2, const basic_vector<T> & u1, const basic_vector<T> & u2, basic_vector<T> * r = 0)
{
	T vvx = v2.x - v1.x,
	  uux = u2.x - u1.x,
	  vvy = v2.y - v1.y,
	  uuy = u2.y - u1.y,
	  vux = v1.x - u1.x,
	  vuy = v1.y - u1.y;
	T d = vvx * uuy - vvy * uux,
	  p = vuy * uux - vux * uuy,
	  q = vuy * vvx - vux * vvy;
	bool result = (d > 0) ? (p >= 0 && p <= d && q >= 0 && q <= d) : 
	                        (p <= 0 && p >= d && q <= 0 && q >= d);
	if (result && r != 0) {
		r->x = v1.x + v2.x * (p / d);
		r->y = v1.y + v2.y * (p / d);
	}
	return result;
}

template <class T>
inline basic_vector<T> intersect1(
	const basic_vector<T> & r1, const basic_vector<T> & dir1, 
	const basic_vector<T> & r2, const basic_vector<T> & dir2)
{ 
	return r1 + (outer_product(dir2, r1 - r2) / outer_product(dir1, dir2)) * dir1; 
}

template <class T>
bool intersect(basic_vector<T> v1, basic_vector<T> v2, const basic_box<T> & b, basic_vector<T> * r1 = 0, basic_vector<T> * r2 = 0)
{
	layout_code c1 = layout(v1, b);
	layout_code c2 = layout(v2, b);
	bool in = (c1 | c2) == 0;
	bool out = (c1 & c2) != 0;
	bool swapped = false;

	while (!in && !out) {

		if (c1 == inside) {
			std::swap(v1, v2);
			std::swap(c1, c2);
			swapped = !swapped;
		}

		if (c1 & left) { // clip left 
			v1.y += (v2.y - v1.y) * (b.x1 - v1.x) / (v2.x - v1.x);
			v1.x = b.x1;
		} else
		if (c1 & below) { // clip below
			v1.x += (v2.x - v1.x) * (b.y1 - v1.y) / (v2.y - v1.y);
			v1.y = b.y1;
		} else
		if (c1 & right) { // clip right
			v1.y += (v2.y - v1.y) * (b.x2 - v1.x) / (v2.x - v1.x);
			v1.x = b.x2;
		} else
		if (c1 & above) { // clip above
			v1.x += (v2.x - v1.x) * (b.y2 - v1.y) / (v2.y - v1.y);
			v1.y = b.y2;
		}

		c1 = layout(v1, b);
		c2 = layout(v2, b);
		in = (c1 | c2) == 0;
		out = (c1 & c2) != 0;
	}

	if (in && r1 && r2) {
		if (!swapped) {
			* r1 = v1;
			* r2 = v2;
		} else {
			* r1 = v2;
			* r2 = v1;
		}
	}

	return in;
}

template <typename T, typename I, typename C> 
T projection(basic_vector<T> v, I first, I last, I & start, T & c, const C & cast) {
	T min = std::numeric_limits<T>::max();
	T result = 0;
	if (first == last) {
		start = last;
		c = 0;
		return result;
	}
	I is, ie;
	for (is = first, ie = first, ++ie; ie != last; ++is, ++ie) {
		basic_vector<T> vs = cast(* is), ve = cast(* ie);
		T d, s, h;
		d = distance(v, vs, ve, &h, &s);
		if (d < min) {
			min = d;
			result = d * (h >= 0 ? 1 : -1);
			start = s < 1 ? is : ie;
			c = s < 1 ? s : 0;
		}
	}
	return result;
}

template <typename T, typename I> 
T projection(basic_vector<T> v, I first, I last, I & start, T & c)
	{ return projection(v, first, last, start, c, xtd::static_typecast< basic_vector<T> >()); }

template <typename T, typename I> 
basic_box<T> bounding_box(I first, I last)
{
	basic_box<T> b(0, 0, 0, 0);
	bool init = true;
	for (I it = first; it != last; ++it)
	{
		const basic_vector<T> & p(* it);
		if (init) {
			b = basic_box<T>(p, p);
			init = false;
		} else 
			b.include(p);
	}
	return b;
}

/**
	Returns the angle (in radians) between two vectors.
	Positive direction is ccw, the range is [0, 2 pi).
	If either of vectors is 0, the result is 0.
 */
template <typename T>
double angle(const basic_vector<T> & v0, const basic_vector<T> & v1) 
{
	T denom = abs(v0) * abs(v1);
	if (denom == 0)
		return 0;
	double a = asin(outer_product(v0, v1) / denom); // [-pi/2, pi/2)
	if (scalar_product(v0, v1) < 0)
		a = pi - a;
	if (a < 0)
		a += 2 * pi;
	return a;
}

/**
	Returns true if vector v lies between v0 and v1
	(in ccw direction)
 */
template <typename T>
bool between(const basic_vector<T> & v0, const basic_vector<T> & v1, const basic_vector<T> & v) 
{
	return angle(v0, v1) >= angle(v0, v);
}

/**
	Performs decomposition of vector v in basis vectors (v0, v1)
 */
template <typename T>
void decompose(const basic_vector<T> & v0, const basic_vector<T> & v1, basic_vector<T> & v)
{
	//the following code was replaced because dependency from mtx library of this common code is very undesired
	//and makes things slower
/*	mtx::fixed<2, 2>::matrix A;
	A(0, 0) = v0.x;
	A(0, 1) = v1.x;
	A(1, 0) = v0.y;
	A(1, 1) = v1.y;
	mtx::fixed<2>::vector b;
	b(0) = v.x;
	b(1) = v.y;
	mtx::sle(A, b);
	v.x = b(0);
	v.y = b(1);*/

	basic_vector<T> w0 = v0.orthogonal();
	basic_vector<T> w1 = v1.orthogonal();
	v = basic_vector<T>(
		scalar_product(v, w1) / scalar_product(v0, w1),
		scalar_product(v, w0) / scalar_product(v1, w0));
}

/**
	Returns true if three vectors are not counter-clockwise oriented
*/
template <typename T>
bool is_clockwise_order(const basic_vector<T> & v0, const basic_vector<T> & v1, const basic_vector<T> & v2)
{
	T prd01 = outer_product(v0, v1);
	T prd02 = outer_product(v0, v2);
	if (prd01 > 0) {
		if (prd02 > 0) {
			T scl01 = scalar_product(v0, v1);
			T scl02 = scalar_product(v0, v2);
			if (scl01 <= 0 && scl02 >= 0)
				return true;
			if (scl01 >= 0 && scl02 <= 0)
				return false;
			if (scl01 > 0) {
				assert(scl02 > 0);
				if (sqr(v2) * sqr(scl01) < sqr(v1) * sqr(scl02))
					return true;
			}
			else {
				assert(scl02 < 0);
				if (sqr(v2) * sqr(scl01) > sqr(v1) * sqr(scl02))
					return true;
			}
		}
	}
	else {
		//prd01 < 0
		if (prd02 > 0)
			return true;
		T scl01 = scalar_product(v0, v1);
		T scl02 = scalar_product(v0, v2);
		if (scl01 >= 0 && scl02 <= 0)
			return true;
		if (scl01 <= 0 && scl02 >= 0)
			return false;
		if (scl01 > 0) {
			assert(scl02 > 0);
			if (sqr(v2) * sqr(scl01) > sqr(v1) * sqr(scl02))
				return true;
		}
		else {
			assert(scl02 < 0);
			if (sqr(v2) * sqr(scl01) < sqr(v1) * sqr(scl02))
				return true;
		}
	}
	return false;
}

/** 
	Projects the point p on the segment (p0, p1);
	degenerate is set to true when the segment is too short (in such case,
	other parameters are not computed, and the function returns false);
	h receives the distance from p to the segment line
	t receives the parameter along the segment line of the projection
	p* = p0 + (p1 - p0) * t;
	Returns true if the projection falls inside the segment
*/
template <typename T>
bool project_point_on_segment(const basic_vector<T> & p, 
	const basic_vector<T> & p0, const basic_vector<T> & p1, 
	bool & degenerate, T & h, T & t);


/**
	Finds intersection of the 2D segments (p0, p1) and (q0, q1);
	collinear is set to true when the segments are nearly collinear (in such case,
	other parameters are not computed, and the function returns false);
	s receives intersection parameter along (p0, p1);
	t receives intersection parameter along (q0, q1);
	Returns true if the intersection of the lines containing the segments lies
	in the relative interior of both segments
*/
template <typename T>
bool intersect_segments(const basic_vector<T> & p0, const basic_vector<T> & p1,
	const basic_vector<T> & q0, const basic_vector<T> & q1,
	bool & collinear, T & s, T & t);

} // g2

#include "algorithm.inl"